#include "ImageMatch.h"
#include "..\MathAux\5point\5point.h"
#include "Util.h"
#include <string>

using namespace cv;

#define RANSAC_THRESHOLD 2.25
#define RANSAC_ROUNDS	512

CImageMatch::CImageMatch(CImageData* ImA, CImageData* ImB, int NumInliners, double InlinersRatio) : m_ImageData(ImA, ImB), 
																									m_NumInliners(NumInliners), 
																									m_InlinersRatio(InlinersRatio)
{
	m_MatchedKeyPoints = Intersect(ImA, ImB);
	m_NumberOfMatches = m_MatchedKeyPoints.size();

	UINT idx1 = m_ImageData.first->GetImageIndex();
	UINT idx2 = m_ImageData.second->GetImageIndex();

	VISIO_ASSERT(idx1 != idx2);

	// will be returned in increasing order
	if (idx1 < idx2)
	{
		m_ImageIndex = ImagePairIndex(idx1, idx2);
	}
	else
	{
		m_ImageIndex = ImagePairIndex(idx2, idx1);
	}
}


CImageMatch::~CImageMatch()
{
	
}

void CImageMatch::NormalizePoints(int n, double* _rPoints,double* _lPoints,double* rPointsNorm,double* lPointsNorm,cv::Mat K1,cv::Mat K2) const
{
	for (int i = 0; i < n; i++) 
	{
		cv::Mat r = INIT_MAT(3, 1, _rPoints[2*i], _rPoints[2*i + 1], 1.0);
		cv::Mat l = INIT_MAT(3, 1, _lPoints[2*i], _lPoints[2*i + 1], 1.0);

		cv::Mat rNorm = K1.inv()*r;
		cv::Mat lNorm = K2.inv()*l;

		cv::Mat rNormScaled = rNorm * -1.0;
		cv::Mat lNormScaled = lNorm * -1.0;

		memcpy(rPointsNorm + 2*i, GetMatrixDataAsDouble(rNormScaled), sizeof(double) * 2);
		memcpy(lPointsNorm + 2*i, GetMatrixDataAsDouble(lNormScaled), sizeof(double) * 2);
	}
}

cv::Mat CImageMatch::CalculateEmatrixInternal(int n, double* _rPoints, double* _lPoints, double ransac_threshold, int ransac_rounds, cv::Mat K1, cv::Mat K2) const
{
	int max_inliers = 0;
	double min_score = DBL_MAX;
	double E_best[9];
	v2_t r_best, l_best;

	double* rPointsNorm = new double[n * 2];
	double* lPointsNorm = new double[n * 2];

	NormalizePoints(n, _rPoints, _lPoints, rPointsNorm, lPointsNorm, K1, K2);
	v2_t* r_pts = CastToBundlerVec<v2_t>(_rPoints);
	v2_t* l_pts = CastToBundlerVec<v2_t>(_lPoints);
	v2_t* r_pts_norm = CastToBundlerVec<v2_t>(rPointsNorm);
	v2_t* l_pts_norm = CastToBundlerVec<v2_t>(lPointsNorm);
	double thresh_norm = ransac_threshold * ransac_threshold;

	for (int round = 0; round < ransac_rounds; round++) 
	{
		/* Pick 5 random points */
		v2_t r_pts_inner[5], l_pts_inner[5];
		int indices[5];
		int num_hyp;
		double E[90];
		int inliers_hyp[10];
		int first_hyp = -1, first_hyp_idx = -1, second_hyp = -1;
		int best = 0;
		int num_ident = 0;
		int inliers = 0;

		choose(n, 5, indices);

		for (int i = 0; i < 5; i++) 
		{
			r_pts_inner[i] = r_pts_norm[indices[i]];
			l_pts_inner[i] = l_pts_norm[indices[i]];

			/* Check for degeneracy */
			if (Vx(r_pts_inner[i]) == Vx(l_pts_inner[i]) && Vy(r_pts_inner[i]) == Vy(l_pts_inner[i]))
			{
				num_ident++;
			}
		}

		if (num_ident >= 3)
		{
			continue;  /* choose another 5 */
		}

		generate_Ematrix_hypotheses(5, r_pts_inner, l_pts_inner, &num_hyp, E);

		for (int i = 0; i < num_hyp; i++) 
		{
			int best_inlier;
			double score = 0.0;

			double E2[9];
			memcpy(E2, E + 9 * i, 9 * sizeof(double));
			E2[0] = -E2[0];
			E2[1] = -E2[1];
			E2[3] = -E2[3];
			E2[4] = -E2[4];
			E2[8] = -E2[8];

			cv::Mat mE2 = INIT_MAT_ARRAY(3, 3, E2);

			cv::Mat mF = K2.inv().t()*mE2*K1.inv();

			inliers = evaluate_Ematrix(n, r_pts, l_pts, // r_pts_norm, l_pts_norm, 
									   thresh_norm, GetMatrixDataAsDouble(mF), // E + 9 * i, 
									   &best_inlier, &score);

			if (inliers > max_inliers || (inliers == max_inliers && score < min_score)) 
			{
					best = 1;
					max_inliers = inliers;
					min_score = score;
					memcpy(E_best, E + 9 * i, sizeof(double) * 9);
					r_best = r_pts_norm[best_inlier];
					l_best = l_pts_norm[best_inlier];
			}

			inliers_hyp[i] = inliers;
		}

		if (best) 
		{
			for (int i = 0; i < num_hyp; i++) 
			{
				if (inliers_hyp[i] > first_hyp) 
				{
					first_hyp = inliers_hyp[i];
					first_hyp_idx = i;
				}
			}

			for (int i = 0; i < num_hyp; i++) 
			{
				if (i != first_hyp_idx && inliers_hyp[i] > second_hyp) 
				{
					second_hyp = inliers_hyp[i];
				}
			}

		}
	}

	if (max_inliers > 0) 
	{
		double E2[9];

		memcpy(E2, E_best, 9 * sizeof(double));

		return INIT_MAT_ARRAY(3, 3, E2);
	}
	else
	{
		VISIO_ASSERT(0);
		return EYE(3);
	}


}
cv::Mat CImageMatch::GetEMatrix() const
{ 
	int NumberOfPoints = (int)GetNumMatches();

	double* lPoints = new double[2 * NumberOfPoints];
	double* rPoints = new double[2 * NumberOfPoints];

	cv::Mat K1 = GetIntrinMatrixForImage(FIRST);
	cv::Mat K2 = GetIntrinMatrixForImage(SECOND);

	GetMatchedPointsAsDouble(lPoints, rPoints);

	cv::Mat E2 = CalculateEmatrixInternal(GetNumMatches(), rPoints, lPoints, RANSAC_THRESHOLD, RANSAC_ROUNDS, K1, K2);
	//cv::Mat E = INIT_MAT(3, 3, 0.29315598287184363, 0.35526468679942580, -0.70222683578478751, 64.837419067075288, 0.83527385472336102, -156.02167605552390, 0.22560276339976415, 168.95786241520739, 1.0000000000000000);
	return E2;
}

ImageDataIntersection CImageMatch::Intersect( CImageData* ImA, CImageData* ImB )
{
	ImageDataIntersection intersection;

	//loop over all visable points in ImA check if ImB sees the same track if so take the respective keypoints
	for (VisablePointItr it = ImA->VisablePointsBegin(); it != ImA->VisablePointsEnd(); it++)
	{
		VisablePointItr MatchedVisablePoint = ImB->FindVisablePoint(it);
		if (MatchedVisablePoint != ImB->VisablePointsEnd())
		{
			//TODO: will be faster over search in the track instead of the image data since he is considrably smaller
			KeyPointIndexMatch newMatchIndex(it->second, MatchedVisablePoint->second);
			intersection.push_back(newMatchIndex);
		}
	}

	return intersection;
}

UINT CImageMatch::GetNumMatches() const
{
	VISIO_ASSERT(m_NumberOfMatches == m_MatchedKeyPoints.size());
	return m_NumberOfMatches;
}

double CImageMatch::GetInlinersScore() const
{

	return (m_InlinersRatio == 0) ? 0 : 1.0 / m_InlinersRatio;
}

ImagePairIndex CImageMatch::GetImageIndices() const
{
	return m_ImageIndex;
}

cvImage CImageMatch::GetImageForTexture( UINT index )
{
	switch (index)
	{
	case FIRST:
		return m_ImageData.first->GetImage();
	case SECOND:
		return m_ImageData.second->GetImage();
	default:
		VISIO_ASSERT(0);
		return ZEROS(1,1);
	}
}



CImageData* CImageMatch::GetImageData( UINT index ) const
{
	switch (index)
	{
	case FIRST:
		return m_ImageData.first;
	case SECOND:
		return m_ImageData.second;
	default:
		VISIO_ASSERT(0);
		return NULL;
	}
}

UINT CImageMatch::GetNumInliners() const
{
	return m_NumInliners;
}

KeyPointMatch CImageMatch::GetKeyPoints(UINT index) const
{
	KeyPointIndexMatch IndexMatch = m_MatchedKeyPoints[index];
	cv::Point2d p = m_ImageData.first->GetKeyPoint(IndexMatch.first);
	cv::Point2d q = m_ImageData.second->GetKeyPoint(IndexMatch.second);
	return KeyPointMatch(p,q);
}

void CImageMatch::GetMatchedPointsAsDouble(double* lPoints, double* rPoints) const
{
	for (UINT i = 0; i < GetNumMatches(); i++)
	{
		KeyPointMatch PointMatch = GetKeyPoints(i);

		rPoints[2*i] = PointMatch.first.x;
		rPoints[2*i + 1] = PointMatch.first.y;

		lPoints[2*i] = PointMatch.second.x;
		lPoints[2*i + 1] = PointMatch.second.y;
	}
}

cv::Mat CImageMatch::GetIntrinMatrixForImage(UINT index) const
{
	CImageData* pImageData = GetImageData(index);
	double KDiag[] = {pImageData->GetFocal(), pImageData->GetFocal(), 1.0} ;
	cv::Mat K = Utils::Diag(3, KDiag);

	return K;
}

KeyPointIndexMatch CImageMatch::GetKeyPointsIndex( UINT index ) const
{
	return m_MatchedKeyPoints[index];
}